from scipy.integrate import odeint
import numpy as np
import matplotlib.pyplot as plt


plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题


m_A = 4866
m_B = 2433
f = 3640
delta_m = 1028.876
omiga = 1.7152
k_3 = 683.4558  # 兴波
k_5 = 80000  # 弹簧劲度系数
k_6 = 10000  # 阻尼系数
ro = 1025
g = 9.8
s = np.pi
l_0 = 0.5  # 弹簧原长
I_1 = 8289.43  # 浮子转动惯量
h_2 = 0.5  # 振子的高度
delta_I = 7001.914
k_7 = 654.3383  # 兴波
k_8 = 250000  # 弹簧劲度系数
k_9 = 1000  # 阻尼系数
k_10 = 8890.7  # 静水恢复力矩
L = 1690


# 计算振子的转动惯量
def I_2(x_1, x_2):
    delta_x = m_B * g / k_5
    l_1 = x_2 - x_1 + l_0 - delta_x
    l_2 = l_1 + h_2
    return 1 / 3 * m_B / h_2 * (l_2 ** 3 - l_1 ** 3)


# 定义一个方程组（微分方程组）
def pfun(y, t):
    y0, y1, y2, y3, y4, y5, y6, y7 = y
    return np.array([
        1 / (m_A + delta_m) * (-k_3 * y0 - ro * g * s * y1 + f * np.cos(omiga * t) + k_5 * (y3 - y1) + k_6 * (y2 - y0)),
        y0,
        1 / m_B * (-k_5 * (y3 - y1) - k_6 * (y2 - y0)),
        y2,
        1 / (I_1 + delta_I) * (-k_7 * y4 - k_10 * y5 + L * np.cos(omiga * t) + k_8 * (y7 - y5) + k_9 * (y6 - y4)),
        y4,
        1 / I_2(y1, y3) * (-k_8 * (y7 - y5) - k_9 * (y6 - y4)),
        y6,
    ])


t = np.arange(0, 150, 0.2)  # 创建自变量序列
soli = odeint(pfun, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], t)  # 求数值解
plt.rc('font', size=16)
plt.rc('font', family='SimHei')
plt.plot(t, soli[:, 1], label="浮子位移")
plt.plot(t, soli[:, 3], label="振子位移")
plt.legend()
plt.show()
plt.plot(t, soli[:, 5], label="浮子偏转角")
plt.plot(t, soli[:, 7], label="振子偏转角")
plt.legend()
plt.show()
plt.plot(t, soli[:, 4], label="浮子偏转角速度")
plt.plot(t, soli[:, 6], label="振子偏转角速度")
plt.legend()
plt.show()
print(soli[50, 1], soli[50, 3], soli[50, 5], soli[50, 7])
print(soli[50, 0], soli[50, 2], soli[50, 4], soli[50, 6])
